#include "Bspline.h"
#include "Hand.h"
#include "opencv.h"
#include "TSLskinSeg.h"
#include "particles.h"
#include "handDet.h"
#include <ctime>

string cascadeName = 
		"lbp_958_c1_18_0.45.xml";
string nestedCascadeName = 
		"lbp_958_c1_18_0.45.xml";

string confile = "config4.txt";
string cntpntfile = "controlPoint2.txt";
string mlinefile = "measuremenline.txt";
string xlm1 = "origin.xml";
string xlm2 = "test.xml";



const int num_particles = 1000;
const float THRESHOLD = 0.01;


int tipsx;
int tipsy;

int main()
{
	CascadeClassifier cascade, nestedCascade;
	double scale = 1;
	bool ROIexist = false;

	if( (!cascade.load( cascadeName )) || (!nestedCascade.load(nestedCascadeName )))
	{
		cerr << "ERROR: Could not load classifier cascade" << endl;
		return -1;
	}

	cv::Rect ROI;
	cv::Rect handROI;

	//Hand hand(confile, cntpntfile, mlinefile);
	Hand hand(xlm1);
	cv::RNG rng(time(NULL));
	particle *particles, *new_particles;

	VideoCapture capture(-1);

	if (!capture.isOpened())
	{
		cout<<"can't open the video camera!"<<endl;
		return -1;
	}

	cv::Mat img;
	cv::Mat BW = Mat::zeros(1280,960,CV_8UC1);
	cv::Mat img_bw(BW,cv::Rect(320,240,640,480));

	int countDetetHand(0), countTracking(0),countWeight(0);
	bool detect = true;

	particle preParitcle;

	bool keep = false;
	Mat showImg;
	namedWindow("result", 0);
	while(1)
	{
		capture>>img;
		if (detect)
		{
			//detect the hand
			handDetction( img, cascade, nestedCascade, scale, ROIexist, ROI);
			cv::flip(img,img,1);
			showImg = img.clone();
//			resize(img, showImg, Size(240, 180));
			imshow("result",showImg);
			if (ROIexist)
			{
				countDetetHand++;
			}
			else
			{
				countDetetHand = 0;
			}
			if (countDetetHand == 3)
			{
				handROI.x = ROI.x + 0.25*ROI.width;
				handROI.y = ROI.y + 0.25*ROI.height;
				handROI.width = 0.5*ROI.width;
				handROI.height = 0.5*ROI.height;
				
				countDetetHand = 0;
				ROIexist = false;
				detect = false;


				//initialize the particles
				cv::Point2f moveHand;
				moveHand.x = handROI.x - hand.GetGravity().x + 0.5*handROI.width;
				moveHand.y = handROI.y - hand.GetGravity().y + 0.5*handROI.height;
				float handScale = 1;

				handScale = (float)ROI.area()/(float)hand.GetArea();

				hand.affineHand(moveHand.x, moveHand.y, handScale, 0);
				preParitcle.w = 0;
				preParitcle.x = hand.GetGravity().x;
				preParitcle.y = hand.GetGravity().y;
				preParitcle.s = 1;
				preParitcle.a = 0;

				particles = init_distribution(hand.GetGravity(), num_particles);

			}
			if( waitKey( 10 ) >= 0 )
				break;
		} 
		else
		{
			TSLskinSegment(img,img_bw);
			medianBlur(img_bw,img_bw,13);
//			imshow("123", img_bw);

			int x, y;
			float s, a;
			if (!keep)
			{
#pragma omp parallel for
				for (int i = 0; i<num_particles; i++)
				{
					Hand hand2(hand);

					particles[i] = transition(particles[i], 640, 480, rng);

					x = cvRound(particles[i].x - hand.GetGravity().x);
					y = cvRound(particles[i].y - hand.GetGravity().y);
					s = particles[i].s;
					a = particles[i].a;
					//std::cout<<"aa = "<<a<<std::endl;
					a = 180*a/PI;
					hand2.affineHand(x, y, s, a);
					particles[i].w = hand2.calWeight(img_bw);

				}
			}

			Hand hand2(hand);
			x = cvRound(preParitcle.x - hand.GetGravity().x);
			y = cvRound(preParitcle.y - hand.GetGravity().y);
			s = preParitcle.s;
			a = preParitcle.a;
			//std::cout<<"aa = "<<a<<std::endl;
			a = 180*a/PI;

			hand2.affineHand(x, y, s, a);

			preParitcle.w = hand2.calWeight(img_bw);

			qsort( particles, num_particles, sizeof( particle ), &particle_cmp );

			particle tmp = particles[0];
			if (preParitcle.w < particles[0].w)
			{
				normalize_weights(particles, num_particles);

				new_particles = resample(particles, num_particles);

				free(particles);
				particles = new_particles;

				preParitcle = tmp;
				//std::cout<<"======="<<endl;
				keep = false;
			}
			else keep = true;


			x = cvRound(preParitcle.x - hand.GetGravity().x);
			y = cvRound(preParitcle.y - hand.GetGravity().y);
			s = preParitcle.s;
			a = preParitcle.a;

			a = 180*a/PI;

			Hand hand3(hand);
			hand3.affineHand(x, y, s, a);

			float angle = -45;
			hand3.affineFinger(Hand::INDEX,1,angle);

			//////////////////////////////



			cv::Point tip;


			tip.x = hand3.Gravity.x;
			tip.x = img.rows - tip.x;
			tip.y = hand3.Gravity.y;

/*

			tip.x = *hand3.TipPoints.ptr<float>(0);
			tip.x = img.rows - tip.x;
			tip.y = *(hand3.TipPoints.ptr<float>(0)+1);
*/








			//cout<<tip.x<<"   "<<tip.y<<endl;

			AngleWeight angleWeight;
			angleWeight.angle = angle;
			angleWeight.weight = hand3.calFingerAngleWeight(img_bw);
			//cout<<"weight:"<<fingerWeight.weight<<endl;

			for (int i = 1; i<90; i++)
			{
				hand3.affineFinger(Hand::INDEX,1,1);
				//hand3.showHand(img,Scalar(255,0,0),2);
				float weight = hand3.calFingerAngleWeight(img_bw);
				//cout<<"weight:"<<weight<<endl;
				if (weight > angleWeight.weight)
				{
					angleWeight.angle = angle + i;
					angleWeight.weight = weight;
				}
			}
			hand3.affineFinger(Hand::INDEX, 1, angleWeight.angle-45);

			ScaleWeight scaleWeight;
			scaleWeight.scale =1;
			scaleWeight.weight = hand3.calFingerScaleWeight(img_bw);

			float preScale = 1;

			for (float i = 0.1; i<1.3; i +=0.1)
			{
				hand3.affineFinger(Hand::INDEX, 1/preScale*i,0);
				preScale = i;

				float weight = hand3.calFingerScaleWeight(img_bw);

				if (weight>scaleWeight.weight)
				{
					scaleWeight.scale = i;
					scaleWeight.weight = weight;
				}
			}

			//cout<<scaleWeight.scale<<endl;
			//////////////////////////////////
			//cout<<scaleWeight.weight<<endl;
			hand3.affineFinger(Hand::INDEX, scaleWeight.scale/preScale,0);
			//hand3.showMeasureLinePoints(img,cv::Scalar(255,0,0),2);
			
			if (countTracking < 10)
			{
				countTracking++;
			}
			else
			{
				float weight = hand3.calWeight(img_bw);
				if (weight < THRESHOLD)
				{
					countWeight ++;
				}
				else
				{
					countWeight = 0;
				}
				if (countWeight >= 3)
				{
					free(particles);
					detect = true;
					countWeight = 0;

				}
				else
				{
					mouse_event(MOUSEEVENTF_MOVE, (tip.x - tipsx) * 1, (tip.y - tipsy) * 1, 0, 0);
					tipsx = tip.x;
					tipsy = tip.y;


					if (scaleWeight.scale < 0.5)
					{
						mouse_event(MOUSEEVENTF_LEFTDOWN, 0, 0, 0, 0);
					}
					else
					{
						mouse_event(MOUSEEVENTF_LEFTUP, 0, 0, 0, 0);
					}




					if (tip.x < 300)
					{
						mouse_event(MOUSEEVENTF_MOVE, -20, 0, 0, 0);
					}
					else if (tip.x > 350)
					{
						mouse_event(MOUSEEVENTF_MOVE, 20, 0, 0, 0);
					}


					if (tip.y < 100)
					{
						mouse_event(MOUSEEVENTF_MOVE, 0, -20, 0, 0);
					}
					else if (tip.y > 380)
					{
						mouse_event(MOUSEEVENTF_MOVE, 0, 20, 0, 0);
					}
				}
				





			}
			hand3.showHand(img,Scalar(255,0,255),2);

			cv::flip(img,img,1);
			//imshow("result",img);

			//imwrite(name,img);

			if (waitKey(2)>=0)
			{
				break;
			}	

			//hand.showHand(img, cv::Scalar(255,0,255),2);
//			resize(img, showImg, Size(240, 180));
			showImg = img.clone();
			imshow("result", showImg);
		}
	}

	//free(new_particles);
	return 0;

}